#include "string.h"		
#include "stdio.h"
#include "delay.h"
#include "sys.h"
#include "oled.h"   //64X128 
#include "EXTI.h"
#include "bmp.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h" 
#include "stm32f10x_tim.h"
#include "stm32f10x_exti.h"
#include "math.h"
#include "mpu9250.h"
#include "usart.h"


unsigned char temp[50];	
int count;
int num = 0;
int MPU;
int cms;
float pitch,roll,yaw; 		
short aacx,aacy,aacz;	
short gyrox,gyroy,gyroz;	
short magX,magY,magZ;	
short MPU_temp; 
short Mag[3];

int main(void)
{
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	
	delay_init();

	OLED_Init();

	uart_init(115200);

	MPU = MPU9250_Init();

	OLED_ShowString(0,4,"MPU_Init:",8,1);	
	OLED_ShowNum(54,4,MPU,2,8,1);	

	MPU = mpu_dmp_init();

	OLED_ShowString(0,5,"mpu_dmp_init:",8,1);	
	OLED_ShowNum(78,5,MPU,2,8,1);

	OLED_Refresh();

	while(MPU9250_Init())
	{
		OLED_ShowString(0,2,(unsigned char *)"MPU9250_Init error",8,1);	
		OLED_Refresh();
	}
	while(mpu_dmp_init())
	{

		OLED_ShowString(0,3,(unsigned char *)"mpu_dmp_init error",8,1);
		OLED_Refresh();
		
	}
	TIM2_Init(10000,7200);

//	OLED_Clear();

//	while(1)
//	{
//		if(mpu_mpl_get_data(&pitch,&roll,&yaw)==0)
//		{ 
//			unsigned char data[10];
//			cms = 0;
//			cms++;

//			MPU_Get_Magnetometer(&magX,&magY,&magZ);	
//			MPU_temp = MPU_Get_Temperature();	
//			MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
//			MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
//			
//			magic_cube();
//			count++;

//			sprintf(data,"pitch:%0.2f",pitch);
//			OLED_ShowString(0,1,(unsigned char *)data,8,1);
//			sprintf(data,"roll:%0.2f",roll);
//			OLED_ShowString(0,2,(unsigned char *)data,8,1);
//			sprintf(data,"yaw:%0.2f",yaw);
//			OLED_ShowString(0,3,(unsigned char *)data,8,1);
////			sprintf(data,"temp:%0.2f",(float)MPU_temp);
////			OLED_ShowString(0,0,(unsigned char *)data,8,1);
//			OLED_Re(1,0,4,80);
//			//OLED_Refresh();
//		}
//	}
}

//void TIM2_IRQHandler()
//{
//	if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET){

//	TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
//    cms = 0;
//	sprintf(temp,"MPU:%d  ",count);
//	OLED_ShowString(88,7,(unsigned char *)temp,8,1);

//	sprintf(temp,"FPS:%d  ",cms++);
//	OLED_ShowString(88,0,(unsigned char *)temp,8,1);		
//	
//	sprintf(temp,"count:%d  ",num++);
//	OLED_ShowString(0,7,(unsigned char *)temp,8,1);
//	count = 0 ;
//	
//	OLED_Refresh();
//	}
//}


